#include "transform3f.h"

float det2( float a00,  float a01, float a10, float a11) 
{
	float m01 = a00*a11 - a10*a01;
	return m01;
}

float det3(float a00, float a01, float a02,
			float a10, float a11, float a12,
			float a20, float a21, float a22)
{
	float m01 = a00*a11 - a10*a01;
	float m02 = a00*a21 - a20*a01;
	float m12 = a10*a21 - a20*a11;

	float m012 = m01*a22 - m02*a12 + m12*a02;
	return m012;
}

transform3f inverse(const transform3f& M)
{
	float w = 1.f/det3(M.t11, M.t12, M.t13, M.t21, M.t22, M.t23, M.t31, M.t32, M.t33 );
	return transform3f(det2( M.t22, M.t23, M.t32, M.t33)*w,   
						-det2( M.t12, M.t13, M.t32, M.t33)*w,  
						det2( M.t12, M.t13, M.t22, M.t23)*w,   
						-det3( M.t12, M.t13, M.t14, M.t22, M.t23, M.t24, M.t32, M.t33, M.t34 )*w,
                        
						-det2( M.t21, M.t23, M.t31, M.t33)*w,    
						det2( M.t11, M.t13, M.t31, M.t33)*w,    
						-det2( M.t11, M.t13, M.t21, M.t23)*w,    
						det3( M.t11, M.t13, M.t14, M.t21, M.t23, M.t24, M.t31, M.t33, M.t34 )*w,
                        
						det2( M.t21, M.t22, M.t31, M.t32)*w,   
						-det2( M.t11, M.t12, M.t31, M.t32)*w,  
						det2( M.t11, M.t12, M.t21, M.t22)*w,   
						-det3( M.t11, M.t12, M.t14, M.t21, M.t22, M.t24, M.t31, M.t32, M.t34 )*w);
}

//free functions
transform3f compute_rot_matrix(const vec3f v, float s, float c)
{
	transform3f output;
	vec3f vn = v * ( 1.f / length(v) );

	float x = vn.x;
	float y = vn.y;
	float z = vn.z;

	output.t11 = x * x + ( 1 - x * x ) * c; 
	output.t12 = x * y * ( 1 - c ) - z * s; 
	output.t13 = x * z * ( 1 - c ) + y * s; 
	output.t14 = 0;

	output.t21 = x * y * ( 1 - c ) + z * s; 
	output.t22 = y * y + ( 1 - y * y ) * c; 
	output.t23 = y * z * ( 1 - c ) - x * s; 
	output.t24 = 0;
 
	output.t31 = x * z * ( 1 - c ) - y * s; 
	output.t32 = y * z * ( 1 - c ) + x * s; 
	output.t33 = z * z + ( 1 - z * z ) * c; 
	output.t34 = 0;

	return output;
}

transform3f compute_translation_matrix(const vec3f v)
{
	transform3f output;

	output.t11 = 1; output.t12 = 0; output.t13 = 0; output.t14 = v.x;
	output.t21 = 0; output.t22 = 1; output.t23 = 0; output.t24 = v.y;
	output.t31 = 0; output.t32 = 0; output.t33 = 1; output.t34 = v.z;

	return output;
}

pnt3f transform(const transform3f& M, const pnt3f p)
{
	return pnt3fc(M.t11 * p.x + M.t12 * p.y + M.t13 * p.z + M.t14,
					M.t21 * p.x + M.t22 * p.y + M.t23 * p.z + M.t24,
					M.t31 * p.x + M.t32 * p.y + M.t33 * p.z + M.t34);
}

vec3f transform(const transform3f& M, const vec3f v)
{
	return vec3fc(M.t11 * v.x + M.t12 * v.y + M.t13 * v.z,
					M.t21 * v.x + M.t22 * v.y + M.t23 * v.z,
					M.t31 * v.x + M.t32 * v.y + M.t33 * v.z);
}

transform3f operator*(const transform3f &A, const transform3f &B)
{
	return transform3f(A.t11*B.t11 + A.t12*B.t21 + A.t13*B.t31,
							A.t11*B.t12 + A.t12*B.t22 + A.t13*B.t32,
							A.t11*B.t13 + A.t12*B.t23 + A.t13*B.t33,
							A.t11*B.t14 + A.t12*B.t24 + A.t13*B.t34 + A.t14,

							A.t21*B.t11 + A.t22*B.t21 + A.t23*B.t31,
							A.t21*B.t12 + A.t22*B.t22 + A.t23*B.t32,
							A.t21*B.t13 + A.t22*B.t23 + A.t23*B.t33,
							A.t21*B.t14 + A.t22*B.t24 + A.t23*B.t34 + A.t24,

							A.t31*B.t11 + A.t32*B.t21 + A.t33*B.t31,
							A.t31*B.t12 + A.t32*B.t22 + A.t33*B.t32,
							A.t31*B.t13 + A.t32*B.t23 + A.t33*B.t33,
							A.t31*B.t14 + A.t32*B.t24 + A.t33*B.t34 + A.t34);
}

transform3f::transform3f()
	:	t11(1), t12(0), t13(0), t14(0),
		t21(0), t22(1), t23(0), t24(0),
		t31(0), t32(0), t33(1), t34(0)
{
}

transform3f::transform3f(float m11, float m12, float m13,
			float m21, float m22, float m23,
			float m31, float m32, float m33)
	: t11(m11), t12(m12), t13(m13), t14(0),
		t21(m21), t22(m22), t23(m23), t24(0),
		t31(m31), t32(m32), t33(m33), t34(0)
{}

transform3f::transform3f(float m11, float m12, float m13, float m14,
			float m21, float m22, float m23, float m24,
			float m31, float m32, float m33, float m34)
	: t11(m11), t12(m12), t13(m13), t14(m14),
		t21(m21), t22(m22), t23(m23), t24(m24),
		t31(m31), t32(m32), t33(m33), t34(m34)
{}